#include "datapacket.h"

DataPacket::DataPacket(QByteArray obtdata){
    int gpsuav = 0;    // uav gps type code
    gpsuavheader.append(QByteArray::fromRawData((char*)&gpsuav, 1));
    int gpscar = 31;    // vehicle type code = 00011111
    gpscarheader.append(QByteArray::fromRawData((char*)&gpscar, 1));
    int img = 227;    // image type code
    imgheader.append(QByteArray::fromRawData((char*)&img, 1));
    int cmd = 252;    // command type code
    cmdheader.append(QByteArray::fromRawData((char*)&cmd, 1));

    datalength = obtdata.size();    // number of chars (bytes) in the string
    data.append(obtdata);
}

QByteArray DataPacket::getData(){
    return data;
}

int DataPacket::getCmd(){
    return cmdcode;
}

float DataPacket::getFlyToLat(){
    return flytolat;
}

float DataPacket::getFlyToLng(){
    return flytolng;
}

void DataPacket::encapsulate(QString type){    // pointer data is edited
    qDebug() << "DataPacket::encapsulate() type : " << type;
    if (type == "gps"){
        packGPS();
    }
    else if (type=="image"){
        packImage();
    }
}

void DataPacket::packGPS(){
    qDebug() << "DataPacket::packGPS() data length variable: " << datalength;
    if (datalength == GPSUAVLENGTH){
        data.prepend(gpsuavheader);
    }
    else if (datalength == GPSCARLENGTH){
        data.prepend(gpscarheader);
    }
}

void DataPacket::unpackGPS(){ // gps packet size = 18 [header(1), lat(4), lng(4), alt(4), vel(4), bat(1)]
    data.remove(0,1);   // remove n=1 byte at pos=0
}

void DataPacket::packImage(){
    imgheader.append(QByteArray::fromRawData((char*)&datalength, 4));
    data.prepend(imgheader);
    qDebug() << "[DataPacket] Image header = " << imgheader.toHex();
}

void DataPacket::unpackCommand(){
    data.remove(0,1);   // remove the type code (header)
    cmdcode = data.at(0);
    if (data.size()==9){
        QByteArray temp;
        temp = data.mid(1,4);
        flytolat = *reinterpret_cast<float*>(&temp);    // fly-to latitude
        temp = data.mid(5,4);
        flytolng = *reinterpret_cast<float*>(&temp);    // fly-to longitude
    }
}
